/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file socketThread.cxx
  \brief class implemenation for internet socket thread receiving data

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/
#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "serialThread.h"

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool verbose;
/*<-End Global Variables-->*/

SerialThread::SerialThread(String name,String dev,String baud) : ThreadedPipe(name)
{
	serial           = NULL;
	Connected_serial = false;
	waitCount_serial = 0;
	badReadCount     = 0;
	devStr           = dev;
	baudStr          = baud;
}

void SerialThread::update() 
{
	if( serial == NULL ) {
		serial = new SerialPort(devStr.c_str(), baudStr.c_str());
		if( serial == NULL ) {
			perror("!! Could not get memory for a new serial port\n");
			return;
		}
	} 

	if( serial->getStatus() != SerialPort::Connected ) {
		if( serial->init() != SerialPort::Connected ){
			//printf("Could not open serial port\n");
			sleep(1);
			return;
		}

		serial->setNonBlocking();
		serial->setLocal();

		Connected_serial = true;
		cout 	<< "Serial " << devStr
			<< ":" << baudStr
			<< ":" << ( serial->isBlocking() ? "BLOCK" : "NON-BLOCK" )
			<< " is open for communication\n";

		serial->flushIO();

		if(!Connected_serial) return;
	}

	int maxFD, val, n=0;
	fd_set readFDs, writeFDs;
	struct timeval timeout;

	/*<------Select FDs------->*/
	FD_ZERO(&readFDs);
	FD_ZERO(&writeFDs);

	FD_SET(serial->getFd(),&readFDs);
	FD_SET(serial->getFd(),&writeFDs);
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
	maxFD = serial->getFd();
	maxFD = (fifo_FDs[INBOARD_RX] > maxFD ? fifo_FDs[INBOARD_RX] : maxFD);
	maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	val = select(maxFD + 1, &readFDs, &writeFDs, NULL, &timeout);

	if( val < 0 ) {
		if (errno != EINTR)
			cout << getName() << "::update() - select, " <<  strerror(errno) << endl;
	}
	else if(val == 0) {
	}
	/*<----One FD is Active--->*/
	else {

		/*<---RX Operations--->*/
		if(FD_ISSET(serial->getFd(), &readFDs) && FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs))
			if((n = read()) > 0) {
				fifo_msg.size = n;
				writeFifo(INBOARD_TX);
			}

		/*<---TX Operations--->*/
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs) && FD_ISSET(serial->getFd(), &writeFDs)){
			if((n = readFifo(INBOARD_RX)) > 0)
				write(n);
		}
	}
	usleep(10000);
}

int SerialThread::read()
{
//	SerialPort::SerialWait serialWait;
	int n=0;

//	serialWait = serial->wait(1000);

//	if( serialWait == SerialPort::WAIT_TIMEOUT) {
//		++waitCount_serial;

//		if( Connected_serial && waitCount_serial > 5) {
//			cout << "<!! Serial port Lost connection !!>" << endl;
//			serial->flushIO(); 
//			Connected_serial = false;
//			waitCount_serial = 0;
//			badReadCount     = 0;
//		}
//	} else if(serialWait == SerialPort::WAIT_INT)
//		cout << "serial->wait interrupted" << endl;
//	else if(serialWait == SerialPort::WAIT_ERROR)
//		cout << "serial->wait error" << endl;
//	else if( serialWait == SerialPort::WAIT_DATA) {

		n = serial->read((char *)fifo_msg.data, FIFO_BUF_SIZE);

		if( n > 0) {
			waitCount_serial = 0;
			badReadCount     = 0;
			Connected_serial = true;

			if(verbose)
				cout << getName() << "::read() - read " << n << " bytes" << endl;
		} else {
			if( n == 0) {

				++badReadCount;
				if( Connected_serial && badReadCount > 10) {
					cout << "<!! Serial port lost connection -- too many bad reads !!>" << endl;
					//serial->flushIO(); 
					serial->close();
					Connected_serial = false;
					waitCount_serial = 0;
					badReadCount     = 0;
				}

			} else if( errno != EWOULDBLOCK || errno != EINTR)
				perror("serial() - bad read from client");
		}
//	}
	return n;
}

int SerialThread::write(int num)
{
	if( serial != NULL ) {
		if( serial->write((char *)fifo_msg.data, num) != num ) 
		{
			perror("serial->write" );
			return -1;
		} else {
			if(verbose)
				cout << getName() << "::write() - wrote " << num << " bytes" << endl;
			return num;
		}
	} else cout << getName() << "::write() - serial instance does not exsist\n";
	return -1;
}

void SerialThread::close()
{
	ThreadedPipe::close();
	if( serial != NULL) {

		serial->close();

		if(verbose)
			serial->printStats();

		delete serial;
		serial = NULL;
	}
}
